"""
边界
判断车辆是否超出边界
"""
import configparser as cfg
from bins.tool import *


class Boundary:
    def __init__(self):
        self.boundary = {
            "edge1": [],
            "edge2": [],
            "edge3": [],
            "edge4": []
        }
        self.pid = {
            "kp": 0,
            "ki": 0,
            "kd": 0,
            "kSteer": 0
        }
        self.setting = None
        self.conf = None

    def load_boundary(self, conf: str = None) -> bool:
        """
        读取边界
        读取conf配置文件中的边界参数
        """
        self.setting = cfg.ConfigParser()
        if conf is None:
            self.conf = "./conf/RobotPi.ini"
        else:
            self.conf = conf
        try:
            self.setting.read(self.conf)
            self.pid["kp"] = float(self.setting.get("PID", "thr_p"))
            self.pid["ki"] = float(self.setting.get("PID", "thr_i"))
            self.pid["kd"] = float(self.setting.get("PID", "thr_d"))
            self.pid["kSteer"] = float(self.setting.get("PID", "str_k"))

            self.boundary["edge1"] = [float(self.setting.get("edge", "edgeX1")), float(self.setting.get("edge", "edgeY1"))]
            self.boundary["edge2"] = [float(self.setting.get("edge", "edgeX2")), float(self.setting.get("edge", "edgeY2"))]
            self.boundary["edge3"] = [float(self.setting.get("edge", "edgeX3")), float(self.setting.get("edge", "edgeY3"))]
            self.boundary["edge4"] = [float(self.setting.get("edge", "edgeX4")), float(self.setting.get("edge", "edgeY4"))]
            return True
        except:
            return False

    def update_boundary(self, new_boundary: list) -> bool:
        """
        更新边界
        通过传入的边界参数更新当前边界参数
        """
        try:
            self.boundary["edge1"][0] = new_boundary[0]
            self.boundary["edge1"][1] = new_boundary[1]
            self.boundary["edge2"][0] = new_boundary[2]
            self.boundary["edge2"][1] = new_boundary[3]
            self.boundary["edge3"][0] = new_boundary[4]
            self.boundary["edge3"][1] = new_boundary[5]
            self.boundary["edge4"][0] = new_boundary[6]
            self.boundary["edge4"][1] = new_boundary[7]

            self.setting.set("edge", "edgeX1", new_boundary[0])
            self.setting.set("edge", "edgeY1", new_boundary[1])
            self.setting.set("edge", "edgeX2", new_boundary[2])
            self.setting.set("edge", "edgeY2", new_boundary[3])
            self.setting.set("edge", "edgeX3", new_boundary[4])
            self.setting.set("edge", "edgeY3", new_boundary[5])
            self.setting.set("edge", "edgeX4", new_boundary[6])
            self.setting.set("edge", "edgeY4", new_boundary[7])
            with open(self.conf, "w+") as f:
                self.setting.write(f)
            return True
        except:
            return False

    def update_pid(self, new_pid: list) -> bool:
        """
        更新PID参数
        通过传入的PID参数更新当前PID参数
        """
        try:
            self.pid["kp"] = new_pid[0]
            self.pid["ki"] = new_pid[1]
            self.pid["kd"] = new_pid[2]
            self.pid["kSteer"] = new_pid[3]

            self.setting.set("PID", "thr_p", new_pid[0])
            self.setting.set("PID", "thr_i", new_pid[1])
            self.setting.set("PID", "thr_d", new_pid[2])
            self.setting.set("PID", "str_k", new_pid[3])
            with open(self.conf, "w+") as f:
                self.setting.write(f)
            return True
        except:
            return False

    def is_outside(self, x: float, y: float) -> bool:
        """
        判断是否超出边界
        通过传入的坐标判断是否超出边界
        """
        point = [x, y]
        intersect = 0
        boundary_list = self.boundary.keys()
        for i in range(len(self.boundary) - 1):
            if is_ray_intersects_segment(point, self.boundary[boundary_list[i]], self.boundary[boundary_list[i + 1]]):
                intersect += 1
        if is_ray_intersects_segment(point, self.boundary[boundary_list[0]], self.boundary[boundary_list[-1]]):
            intersect += 1
        if intersect % 2 == 0:
            return True
        return False